'''
Author: Jiagang Chen
Date: 2021-11-09 11:43:51
LastEditors: Jiagang Chen
LastEditTime: 2021-11-12 15:21:14
Description: 
Reference: 
'''

import helper
import tool4kml

cam_focal = 35 # mm
cam_photo_width = 35.9 # mm
cam_photo_height = 24 # mm
cam_video_width = 34 # mm
cam_video_height = 19 # mm

def down_view_photo_traj_gen(wgs84_gps_corner_coords, heading_overlap = 80, side_overlap = 70, fly_height_abs = 200):
    range_x = fly_height_abs / cam_focal * cam_photo_height
    overlap_x = heading_overlap / 100
    range_y = fly_height_abs / cam_focal * cam_photo_width
    overlap_y = side_overlap / 100

    mean_longitude = 0
    for pt in wgs84_gps_corner_coords:
        mean_longitude = mean_longitude + pt[1]
    mean_longitude = mean_longitude / len(wgs84_gps_corner_coords)

    L0 = int((mean_longitude + 1.5) / 3.0) * 3

    proj_gps_corner_coords = []
    for pt in wgs84_gps_corner_coords:
        x, y = helper.LatLon2XY(pt[0], pt[1])
        proj_gps_corner_coords.append([x, y])
    [rect, box_points] = helper.cal_min_rect(proj_gps_corner_coords)
    final_waterpoints_proj_coords = helper.get_waterpoint_from_rect(rect, proj_gps_corner_coords, range_x, overlap_x, range_y, overlap_y)

    final_waterpoints_wgs84_coords = []
    for pt in final_waterpoints_proj_coords:
        Lat, Lon = helper.XY2LatLon(pt[0], pt[1], L0)
        final_waterpoints_wgs84_coords.append([Lat, Lon, fly_height_abs])

    return final_waterpoints_wgs84_coords

def down_view_photo_traj_gen_example(wgs84_gps_corner_coords):
    final_waterpoints_wgs84_coords = down_view_photo_traj_gen(wgs84_gps_corner_coords)

    waypoint_list = []
    for i in range(len(final_waterpoints_wgs84_coords)):
        action = ""
        if i == 0:
            action = "SHOOT.REC"
        if i == len(final_waterpoints_wgs84_coords)-1:
            action = "STOPREC.SHOOT"
        wpt = final_waterpoints_wgs84_coords[i] 
        text_final_waterpoints = ["waypoint_" + str(i), "{:.14f}".format(wpt[1]), "{:.14f}".format(wpt[0]), "{:.1f}".format(wpt[2]), "{:.1f}".format(0), "{:.1f}".format(-90.0), "{:.1f}".format(10.0), "AUTO", action]
        waypoint_list.append(text_final_waterpoints)

    return waypoint_list

def test(wgs84_gps_corner_coords, h):
    waypoint_list = []
    for i in range(len(wgs84_gps_corner_coords)):
        action = ""
        if i == 0:
            action = "SHOOT.REC"
        if i == len(wgs84_gps_corner_coords)-1:
            action = "STOPREC.SHOOT"
        wpt = wgs84_gps_corner_coords[i] 
        text_final_waterpoints = ["waypoint_" + str(i), "{:.14f}".format(wpt[1]), "{:.14f}".format(wpt[0]), "{:.1f}".format(h), "{:.1f}".format(0), "{:.1f}".format(-90), "{:.1f}".format(10.0), "AUTO", action]
        waypoint_list.append(text_final_waterpoints)

    return waypoint_list

def get_gps_corner_point(str_pt):
    wgs84_gps_corner_coords = []
    pts = str_pt.split(' ')
    for pt in pts:
        coords = pt.split(',')
        wgs84_gps_corner_coords.append([float(coords[1]), float(coords[0])])
    return wgs84_gps_corner_coords


if __name__ == "__main__":
    # # https://www.qvdv.com/tools/qvdv-coordinate.html\
    # wgs84_gps_corner_coords = [[30.539407, 114.364176], [30.536405, 114.371497], [30.531016, 114.368821], [30.531211, 114.367868], 
    #                             [30.530783, 114.367832], [30.531452, 114.365838], [30.532284, 114.365973], [30.5323, 114.363979],
    #                             [30.533256, 114.364015], [30.533124, 114.361212], [30.53464, 114.360924], [30.538062, 114.363035]]
    # tool4kml.list2kml(test(wgs84_gps_corner_coords, 150), "test", r"C:\Users\Administrator\Desktop\DJIFly\examples")
    
    # str_pt = "114.3521495365503,30.535395227067863,100.0 114.35089330120708,30.534337146351195,100.0 114.35075549184968,30.53269295124243,100.0 114.34929039186889,30.531351133922353,100.0 114.34897036245968,30.530642599912536,100.0 114.34915261644377,30.529375449287617,100.0 114.34978343860797,30.529258934234598,100.0 114.35202685002255,30.529232240209122,100.0 114.35209926091441,30.528267827166285,100.0 114.35401195134733,30.528283330833695,100.0 114.35411784571656,30.527395080833752,100.0 114.35671521667568,30.526566612185526,100.0 114.35818182069637,30.529287650596657,100.0 114.35818970938585,30.530995633803418,100.0 114.35858489473479,30.531018168749192,100.0 114.35855268227687,30.531722651246024,100.0 114.35827000813532,30.531889345139916,100.0 114.35841326428043,30.53283838837652,100.0 114.35581844761371,30.533673610777885,100.0 114.35283668801027,30.5349484102787,100.0"
    # wgs84_gps_corner_coords = get_gps_corner_point(str_pt)
    # tool4kml.list2kml(down_view_photo_traj_gen_example(wgs84_gps_corner_coords), "whu_wc", r"C:\Users\Administrator\Desktop\DJIFly\examples")

    str_pt = "114.35092314560087,30.529990414846935,100.0 114.35163294679421,30.530677560060628,100.0 114.35204672092812,30.5317555039124,100.0 114.35255829774073,30.532579229792024,100.0 114.35304177778835,30.533122383738778,100.0 114.35340604230583,30.533801988528403,100.0 114.35383066313669,30.533588853559774,100.0 114.35384011557217,30.532713507122274,100.0 114.35284500138019,30.531771573490076,100.0 114.35228252971613,30.530880937157022,100.0 114.35183925058824,30.53021876682093,100.0 114.35143481567289,30.52958194053309,100.0 114.35204558139566,30.529462024015338,100.0 114.3529228243776,30.530199742565756,100.0 114.3534437441175,30.53153390744959,100.0 114.35418305105289,30.532306492101682,100.0 114.35478307134296,30.53302039934773,100.0 114.35542336952898,30.532781547552638,100.0 114.35515553550358,30.53202670963635,100.0 114.354239411978,30.531118106319767,100.0 114.35367691343633,30.530303663758357,100.0 114.35325371363345,30.529599869525793,100.0 114.35300729167994,30.52923532279678,100.0 114.35380563693423,30.528732884903206,100.0 114.35469231253826,30.529283547428935,100.0 114.3559137845838,30.5310243535747,100.0 114.35632763429258,30.531770905846418,100.0 114.35665309762412,30.532297018711127,100.0 114.35755866402774,30.53208321745806,100.0 114.35692643203933,30.531550613261683,100.0 114.35617770241058,30.530669432252306,100.0 114.35570360592189,30.52958228549506,100.0 114.35524153780848,30.528877368194053,100.0 114.35494554928253,30.528427422410058,100.0"
    tool4kml.list2kml(test(get_gps_corner_point(str_pt), 200), "test_video", r"C:\Users\Administrator\Desktop\DJIFly\examples")
    